{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Q-Table One"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Q-table One is used to void the obstacles automatically.\n",
    "### Columns:Nearest|Near|Medium|Far\n",
    "##### Columns register the states\n",
    "### Rows:Up|Down|Turn_left_45 degree|Turn_right_45_degree\n",
    "##### Rows register the actions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120])\n",
    "Robot_radium=40\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Safe distance=1cm\n",
    "Q_table1_states=np.array(['L0R0U0','L0R0U1','L0R0U2','L0R0U3',\n",
    "                'L0R1U0','L0R1U1','L0R1U2','L0R1U3',\n",
    "                'L0R2U0','L0R2U1','L0R2U2','L0R2U3',\n",
    "                'L0R3U0','L0R3U1','L0R3U2','L0R3U3',\n",
    "                'L1R0U0','L1R0U1','L1R0U2','L1R0U3',\n",
    "                'L1R1U0','L1R1U1','L1R1U2','L1R1U3',\n",
    "                'L1R2U0','L1R2U1','L1R2U2','L1R2U3',\n",
    "                'L1R3U0','L1R3U1','L1R3U2','L1R3U3',\n",
    "                'L2R0U0','L2R0U1','L2R0U2','L2R0U3',\n",
    "                'L2R1U0','L2R1U1','L2R1U2','L2R1U3',\n",
    "                'L2R2U0','L2R2U1','L2R2U2','L2R2U3',\n",
    "                'L2R3U0','L2R3U1','L2R3U2','L2R3U3',\n",
    "                'L3R0U0','L3R0U1','L3R0U2','L3R0U3',\n",
    "                'L3R1U0','L3R1U1','L3R1U2','L3R1U3',\n",
    "                'L3R2U0','L3R2U1','L3R2U2','L3R2U3',\n",
    "                'L3R3U0','L3R3U1','L3R3U2','L3R3U3'])\n",
    "Q_table1_actions=np.array(['Up','Down','Left_45D','Right_45D'])\n",
    "Q_table1_actions_length=len(Q_table1_actions)\n",
    "Q_table1_states_length=len(Q_table1_states)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table_real=np.zeros((Q_table1_states_length,Q_table1_actions_length))\n",
    "Q_table_real=pd.DataFrame(Q_table_real,columns=Q_table1_actions,index=Q_table1_states)\n",
    "Q_table_real.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Initial_Q_Table(LengthOfActions,LengthOfStates):\n",
    "    Q_Table=np.zeros((LengthOfStates,LengthOfActions))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map1():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255)) \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "#     draw.ellipse((1500-20,300-20,1500+20,300+20), (255,0,0), (255,0,0))\n",
    "    return im\n",
    "# im_show=Draw_map1()\n",
    "# plt.imshow(im_show)\n",
    "# plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map2():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.rectangle((1500,1000,1600,1100),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,600,900),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,500,600),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1800,800),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1600,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,960,1460),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,500,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,300,1300),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1300,450),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 950, 1900), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,850,300),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1900,330),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "# im_show=Draw_map2()\n",
    "# plt.imshow(im_show)\n",
    "# plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map3():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.polygon((1150, 1180, 1200, 1180, 1250, 1120, 1230, 1090, 1130, 1100),(0,0,0),(0,0,0))\n",
    "    draw.polygon((400,450,600,750,500,800),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1500,750,1700,950),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((300,1500,1000,1750),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((1450,1600,1600,1750),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1200,300,1400,500),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((500,1000,700,1200),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((750,900,700,1200),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "im_show=Draw_map3()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Function Scan is used to detect the distance between obstacles and robot.\n",
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Furthest scanning distance is defined as 290cm\n",
    "def Scan(Current_x,Current_y,Angle,im):\n",
    "    Distance_level=0\n",
    "    Obstacle_distance=Robot_radium\n",
    "    Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "    Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    while(im.getpixel((Obstacle_distance_x,Obstacle_distance_y))!=(0,0,0) and Obstacle_distance<250):#getpixiel obtains the degree of Gray Scale\n",
    "        Obstacle_distance+=5  #Search interval, can be changed\n",
    "        Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "        Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    if 0<=Obstacle_distance<90:\n",
    "        Distance_level=0 #Nearear\n",
    "    elif 90<=Obstacle_distance<170:\n",
    "        Distance_level=1 #Near\n",
    "    elif 170<=Obstacle_distance<250:\n",
    "        Distance_level=2 #Medium\n",
    "    else:\n",
    "        Distance_level=3 #Far\n",
    "    return Distance_level"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Distance=np.arange(0,50,5)\n",
    "    for i in Distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if im.getpixel((x,y))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Random_start(im):\n",
    "    Angle=np.random.random()*360\n",
    "    x,y=np.random.random(2)*2000\n",
    "    while(Is_Crash(x,y,im)==True):\n",
    "        x,y=np.random.random(2)*2000\n",
    "    return x,y,Angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Direction_min_level(Degree,Current_x,Current_y,Current_angle,im):\n",
    "    Level=[]\n",
    "    Degree=Degree+Current_angle\n",
    "    for i in Degree:\n",
    "        Level.append(Scan(Current_x,Current_y,i,im))\n",
    "    return min(Level)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(Left_min,Right_min,Up_min):\n",
    "    LRU=[]\n",
    "    LRU.append(Left_min)\n",
    "    LRU.append(Right_min)\n",
    "    LRU.append(Up_min)\n",
    "    return LRU[0]*16+LRU[1]*4+LRU[2]\n",
    "# print(Output_state_index(Left_min,Right_min,Up_min))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Choose_action is used to selection an action during the training process. It is based on the greedy strategy, if the random \n",
    "#chosen float(0-1) is inferior to current epsilon, robot choose random action to explore, if not, choose maximun Q value\n",
    "#action based on Q Table, more precisely based on the action-state range\n",
    "def Choose_action(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Q_table1_actions_length)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_next_state(Current_x,Current_y,Current_angle,Action,im):\n",
    "    Reward=1\n",
    "    Crash=False\n",
    "    if Action==0:\n",
    "        Next_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "        else:\n",
    "            Reward=2\n",
    "    elif Action==1:\n",
    "        Next_x=Current_x-Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y-Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "    elif Action==2:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle-45\n",
    "    elif Action==3:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle+45        \n",
    "    return Next_x,Next_y,Next_angle,Reward,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x,Vec_y):\n",
    "    im=Image.new(\"RGB\", size=(2000,2000),color=(0,0,0))\n",
    "    draw = ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((Vec_x[0]-40,Vec_y[0]-40, Vec_x[0]+40,Vec_y[0]+40),(0,255,0),(0,255,0)) \n",
    "    for i,j in zip(Vec_x[1:],Vec_y[1:]):\n",
    "        draw.ellipse((i-40,j-40, i+40,j+40),(0,255,0),(0,255,0)) \n",
    "    return im"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table1_gait=np.loadtxt(r\"E:\\Graduate\\python\\Q_Table_notgait_finish\\Q_Table1_notgait\\Q_Table1_notgait_1.txt\")\n",
    "Q_table1_gait=pd.DataFrame(Q_table1_gait,columns=Q_table1_actions,index=Q_table1_states)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def test(Q_Table):\n",
    "    Crash=False\n",
    "    im=Draw_map1()\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    Test_time=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    while Test_time<100001:\n",
    "        Current_left_obstacle_level=Direction_min_level(Left_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_right_obstacle_level=Direction_min_level(Right_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_obstacle_level=Direction_min_level(Up_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state=Output_state_index(Current_left_obstacle_level,Current_right_obstacle_level,Current_up_obstacle_level)\n",
    "        Next_action=np.argmax(Q_Table[Current_state])\n",
    "#         print(\"------------------------------------------------------------------------------------------\")\n",
    "#         print('Current_x=%f   Current_y=%f   Current_angle=%f'%(Current_x,Current_y,(Current_angle%360)))\n",
    "        Next_x,Next_y,Next_angle,Reward,Crash=Output_next_state(Current_x,Current_y,Current_angle,Next_action,im)\n",
    "#         print('Current_state=%d'%Current_state)\n",
    "#         print('Next_action=%d'%Next_action)\n",
    "#         print('Next_x=%f   Next_y=%f   Next_angle=%f'%(Next_x,Next_y,(Next_angle%360)))\n",
    "        \n",
    "        Vec_x.append(Current_x)\n",
    "        Vec_y.append(Current_y)\n",
    "        if Crash==True:\n",
    "            print('Boom')\n",
    "            print(Test_time)\n",
    "            break\n",
    "        else:\n",
    "            if(Test_time%10000==0):\n",
    "                print(\"******************\")\n",
    "                print(Test_time)\n",
    "                im_show=Movement_plot(Vec_x,Vec_y)\n",
    "                print(\"******************\")\n",
    "                plt.imshow(im_show)\n",
    "                plt.show()\n",
    "                Vec_x=[]\n",
    "                Vec_y=[]\n",
    "                Next_x,Next_y,Current_a=Random_start(im)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "        Test_time+=1\n",
    "test(Q_table1_gait.as_matrix())\n",
    "# np.savetxt(\"Q_Table_try.txt\",Q_Table_Final.as_matrix())\n",
    "            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table_Final.iloc[30,:]"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
